﻿#pragma once 

#include "CoreMinimal.h"

namespace BigMath
{
    struct FInt128
    {
    public:
        uint64 Low;
        int64 High;

    public:
        static constexpr uint64 Base32 = 0x100000000;
        static constexpr uint64 NegativeSignMask = 0x1ull << 63;
        
    public:
        FInt128() : Low(0), High(0) {};
        FInt128(int64 Value)
        {
            High = static_cast<uint64>(Value < 0 ? ~0 : 0);
            Low = static_cast<uint64>(Value);
        }

        FInt128(const int64& InHigh, const uint64& InLow) : Low(InLow), High(InHigh) {};
        
    private:
        FORCEINLINE FInt128(int32 Sigh, const uint32* IntList, int32 IntLen);

    public:
        explicit FORCEINLINE operator int64() const;
        
    public:
        FORCEINLINE int32 Sign() const;
        
    private:
        FORCEINLINE static FInt128 Multiply(FInt128 Left, FInt128 Right);
        FORCEINLINE static FInt128 DivRem(FInt128 Dividend, FInt128 Divisor, FInt128& Remainder);

    private:
        FORCEINLINE void Negate();
        FORCEINLINE static int32 GetNormalizeShift(uint32 Value);
        FORCEINLINE static int32 GetLength(const uint32* UIntList, int32 UIntListLen);
        FORCEINLINE static void Normalize(const uint32* U, int32 L, uint32* UN, int32 Shift);
        FORCEINLINE static void Unnormalize(const uint32* UN, uint32* R, int32 Shift);
        FORCEINLINE static void DivModUnsigned(uint32* U, uint32* V, uint32*& Q, uint32*& R);
        
    public:
        FORCEINLINE friend bool operator == (const FInt128& Left, const FInt128& Right);
        FORCEINLINE FInt128 operator - () const;
        FORCEINLINE friend FInt128 operator + (const FInt128& Left, const FInt128& Right);
        FORCEINLINE FInt128& operator += (const FInt128& Value);
        FORCEINLINE friend FInt128 operator / (const FInt128& Left, const FInt128& Right);
        FORCEINLINE friend FInt128 operator * (const FInt128& Left, const FInt128& Right);
        FORCEINLINE friend FInt128 operator << (const FInt128& Value, int32 Shift);
        FORCEINLINE friend FInt128 operator >> (const FInt128& Value, int32 Shift);
    };

    FORCEINLINE FInt128::FInt128(int32 Sigh, const uint32* IntList, int32 IntLen)
    {
        uint64 Value[2];
        FMemory::Memset(Value, 0, sizeof(Value));
        for (int32 I = 0; I < IntLen && I < 4; ++I)
        {
            FMemory::Memcpy((I * 4) + reinterpret_cast<int8*>(Value), IntList + I, 4);
        }

        High = Value[1];
        Low = Value[0];

        if (Sigh < 0 && (High > 0 || Low > 0))
        {
            Negate();
            High |= NegativeSignMask;
        }
    }

    FORCEINLINE FInt128::operator int64() const
    {
        return static_cast<int64>(Low);
    }

    FORCEINLINE int32 FInt128::Sign() const
    {
        if (High == 0 && Low == 0)
        {
            return 0;
        }
        return ((High & NegativeSignMask) == 0) ? 1 : -1;
    }

    FORCEINLINE void FInt128::Negate()
    {
        High = ~High;
        Low = ~Low;
        (*this) += 1;
    }

    FORCEINLINE FInt128 FInt128::Multiply(FInt128 Left, FInt128 Right)
    {
        const int32 LeftSign = Left.Sign();
        Left = LeftSign < 0 ? -Left : Left;

        const int32 RightSign = Right.Sign();
        Right = RightSign < 0 ? -Right : Right;

        uint32 XInts[4];
        uint32 YInts[4];
        FMemory::Memcpy(XInts, &Left.Low, 16);
        FMemory::Memcpy(YInts, &Right.Low, 16);

        uint32 MulIntList[8] = { 0 };
        for (int32 I = 0; I < 4; ++I)
        {
            int32 Index = I;
            uint64 Remainder = 0;
            for (int32 J = 0; J < 4; ++J)
            {
                const uint32 Yi = YInts[J];
                Remainder = Remainder + static_cast<uint64>(XInts[I]) * Yi + MulIntList[Index];
                MulIntList[Index++] = static_cast<uint32>(Remainder);
                Remainder = Remainder >> 32;
            }

            while (Remainder != 0)
            {
                Remainder += MulIntList[Index];
                MulIntList[Index++] = static_cast<uint32>(Remainder);
                Remainder = Remainder >> 32;
            }
        }

        return FInt128(LeftSign * RightSign, MulIntList, 8);
    }

    FORCEINLINE FInt128 FInt128::DivRem(FInt128 Dividend, FInt128 Divisor, FInt128& Remainder)
    {
        if (Divisor == 0 || Dividend == 0)
        {
            return 0; // DivideByZeroException
        }

        const int32 DividendSign = Dividend.Sign();
        Dividend = DividendSign < 0 ? -Dividend : Dividend;

        const int32 DivisorSign = Divisor.Sign();
        Divisor = DivisorSign < 0 ? -Divisor : Divisor;

        uint32 AQuotient[4] = { 0 };
        uint32 ARem[4] = { 0 };

        uint32* Quotient = AQuotient;
        uint32* Rem = ARem;
        uint32* U = reinterpret_cast<uint32*>(&Dividend.Low);
        uint32* V = reinterpret_cast<uint32*>(&Divisor.Low);
        FInt128::DivModUnsigned(U, V, Quotient, Rem);

        Remainder = FInt128(1, Rem, 4);
        return FInt128(DividendSign * DivisorSign, Quotient, 4);
    }

    FORCEINLINE FInt128 FInt128::operator - () const
    {
        FInt128 Temp = *this;
        Temp.Negate();
        return Temp;
    }

    FORCEINLINE bool operator==(const FInt128& Left, const FInt128& Right)
    {
        return (Left.Low == Right.Low) && (Left.High == Right.High);
    }

    FORCEINLINE FInt128 operator + (const FInt128& Left, const FInt128& Right)
    {
        FInt128 Value;
        Value.High = Left.High + Right.High;
        Value.Low = Left.Low + Right.Low;
        // Carry
        if (Value.Low < Left.Low)
        {
            Value.High++;
        }
        return Value;
    }

    FORCEINLINE FInt128& FInt128::operator += (const FInt128& Value)
    {
        *this = *this + Value;
        return *this;
    }

    inline FInt128 operator/(const FInt128& Left, const FInt128& Right)
    {
        FInt128 Remainder = 0;
        return FInt128::DivRem(Left, Right, Remainder);
    }

    FORCEINLINE FInt128 operator * (const FInt128& Left, const FInt128& Right)
    {
        return FInt128::Multiply(Left, Right);
    }

    FORCEINLINE FInt128 operator << (const FInt128& Value, int32 Shift)
    {
        if (Shift == 0 || Value == 0)
        {
            return Value;
        }

        if (Shift < 0)
        {
            return Value >> -Shift;
        }

        const uint64* Values = const_cast<uint64*>(&Value.Low);

        Shift = Shift % 128;

        const int32 ShiftOffset = Shift / 64;
        const int32 BShift = Shift % 64;

        uint64 Shifted[2];
        FMemory::Memset(Shifted, 0, sizeof(Shifted));

        for (int32 I = 0; I < 2; ++I)
        {
            const int32 IShit = I + ShiftOffset;
            if (IShit >= 2)
            {
                continue;
            }

            Shifted[IShit] |= Values[I] << BShift;
            if (BShift > 0 && I - 1 >= 0)
            {
                Shifted[IShit] |= Values[I - 1] >> (64 - BShift);
            }
        }

        return FInt128(static_cast<int64>(Shifted[1]), Shifted[0]); // low is stored in array entry 0  
    }

    FORCEINLINE FInt128 operator >> (const FInt128& Value, int32 Shift)
    {
        if (Shift == 0 || Value == 0)
        {
            return Value;
        }

        if (Shift < 0)
        {
            return Value << -Shift;
        }

        uint64* values = const_cast<uint64*>(&Value.Low);
        Shift = Shift % 128; // This is the defined behavior of shift. Shifting by greater than the number of bits uses a mod

        //
        //  First, shift over by full ulongs. This could be optimized a bit for longer arrays (if shifting by multiple longs, we do more copies 
        //  than needed), but for short arrays this is probably the best way to go
        //
        while (Shift >= 64)
        {
            for (int32 I = 0; I < 1; ++I)
            {
                values[I] = values[I + 1];
            }

            values[1] = static_cast<uint64>(static_cast<int64>(values[1]) >> (64 - 1));    // Preserve sign of upper long, will either be 0 or all f's
            Shift -= 64;
        }

        //
        //  Now, we just have a sub-long shift left to do (shift will be < 64 at this point)
        //
        if (Shift == 0)
        {
            return Value;
        }

        const int32 BShift = 64 - Shift;

        //
        //  In right shifting, upper val is a special case because we need to preserve the sign bits, and because we don't need to or in
        //  any other values
        //
        uint64 Shifted[2];
        FMemory::Memset(Shifted, 0, sizeof(Shifted));

        Shifted[1] = static_cast<uint64>(static_cast<int64>(values[1]) >> Shift);    // Preserve sign of upper long
        for (int32 I = 0; I < 1; ++I)
        {
            Shifted[I] = values[I] >> Shift;                   // Unsigned, so upper bits stay zero
            Shifted[I] |= (values[I + 1] << BShift);
        }

        return FInt128(static_cast<int64>(Shifted[1]), Shifted[0]); // lo is stored in array entry 0  
    }

    FORCEINLINE int32 FInt128::GetNormalizeShift(uint32 Value)
    {
        int32 Shift = 0;

        if ((Value & 0xFFFF0000) == 0)
        {
            Value <<= 16;
            Shift += 16;
        }
        
        if ((Value & 0xFF000000) == 0)
        {
            Value <<= 8;
            Shift += 8;
        }
        
        if ((Value & 0xF0000000) == 0)
        {
            Value <<= 4;
            Shift += 4;
        }
        
        if ((Value & 0xC0000000) == 0)
        {
            Value <<= 2;
            Shift += 2;
        }
        
        if ((Value & 0x80000000) == 0)
        {
            Shift += 1;
        }

        return Shift;
    }

    FORCEINLINE int32 FInt128::GetLength(const uint32* UIntList, int32 UIntListLen)
    {
        int32 Index = UIntListLen - 1;
        while ((Index >= 0) && (UIntList[Index] == 0))
            Index--;
        Index = Index < 0 ? 0 : Index;
        return Index + 1;
    }
    
    FORCEINLINE void FInt128::Normalize(const uint32* U, int32 L, uint32* UN, int32 Shift)
    {
        uint32 Carry = 0;
        int32 I;
        if (Shift > 0)
        {
            const int32 RShift = 32 - Shift;
            for (I = 0; I < L; ++I)
            {
                const uint32 UI = U[I];
                UN[I] = (UI << Shift) | Carry;
                Carry = UI >> RShift;
            }
        }
        else
        {
            for (I = 0; I < L; ++I)
            {
                UN[I] = U[I];
            }
        }

        while (I < 4)
        {
            UN[I++] = 0;
        }

        if (Carry != 0)
        {
            UN[L] = Carry;
        }
    }

    FORCEINLINE void FInt128::Unnormalize(const uint32* UN, uint32* R, int32 Shift)
    {
        constexpr int32 Length = 4;
        if (Shift > 0)
        {
            const int32 LShift = 32 - Shift;
            uint32 Carry = 0;
            for (int32 I = Length - 1; I >= 0; I--)
            {
                const uint32 UNI = UN[I];
                R[I] = (UNI >> Shift) | Carry;
                Carry = (UNI << LShift);
            }
        }
        else
        {
            for (int32 I = 0; I < Length; I++)
            {
                R[I] = UN[I];
            }
        }
    }
    
    FORCEINLINE void FInt128::DivModUnsigned(uint32* U, uint32* V, uint32*& Q, uint32*& R)
    {
        const int32 M = GetLength(U, 4);
        const int32 N = GetLength(V, 4);

        if (N <= 1)
        {
            //  Divide by single digit
            //
            uint64 Rem = 0;
            const uint32 V0 = V[0];

            for (int32 J = M - 1; J >= 0; J--)
            {
                Rem *= Base32;
                Rem += U[J];

                const uint64 Div = Rem / V0;
                Rem -= Div * V0;
                Q[J] = static_cast<uint32>(Div);
            }
            
            R[0] = static_cast<uint32>(Rem);
        }
        else if (M >= N)
        {
            const int32 Shift = GetNormalizeShift(V[N - 1]);

            uint32 UN[4] = { 0 };
            uint32 VN[4] = { 0 };

            Normalize(U, M, UN, Shift);
            Normalize(V, N, VN, Shift);

            //  Main division loop
            //
            for (int32 J = M - N; J >= 0; J--)
            {
                int32 I;

                uint64 RR = Base32 * UN[J + N] + UN[J + N - 1];
                uint64 QQ = RR / VN[N - 1];
                RR -= QQ * VN[N - 1];

                for (;;)
                {
                    // Estimate too big ?
                    //
                    if ((QQ >= Base32) || (QQ*VN[N - 2] > (RR*Base32 + UN[J + N - 2])))
                    {
                        QQ--;
                        RR += VN[N - 1];
                        if (RR < Base32)
                        {
                            continue;
                        }
                    }
                    break;
                }

                //  Multiply and subtract
                //
                int64 B = 0;
                int64 T;
                for (I = 0; I < N; I++)
                {
                    uint64 P = VN[I] * QQ;
                    T = UN[I + J] - static_cast<int64>(static_cast<uint32>(P)) - B;
                    UN[I + J] = static_cast<uint32>(T);
                    P >>= 32;
                    T >>= 32;
                    B = static_cast<int64>(P) - T;
                }
                T = UN[J + N] - B;
                UN[J + N] = static_cast<uint32>(T);

                //  Store the calculated value
                //
                Q[J] = static_cast<uint32>(QQ);

                //  Add back vn[0..n] to un[j..j+n]
                //
                if (T < 0)
                {
                    Q[J]--;
                    uint64 C = 0;
                    for (I = 0; I < N; I++)
                    {
                        C = static_cast<uint64>(VN[I]) + UN[J + I] + C;
                        UN[J + I] = static_cast<uint32>(C);
                        C >>= 32;
                    }
                    C += UN[J + N];
                    UN[J + N] = static_cast<uint32>(C);
                }
            }

            Unnormalize(UN, R, Shift);
        }
        else
        {
            FMemory::Memset(Q, 0, 16);
            FMemory::Memcpy(R, U, 16);
        }
    }
}
